/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package Threads;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVision;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
import ticks.chromedome.commands.CommandBase;

/**
 *
 * @author Jared
 */
public class VisionProcessing implements Runnable {
   AxisCamera camera;          // the axis camera object (connected to the switch)
   CriteriaCollection cc;      // the criteria for doing the particle filter operation
   public double[] coordinates;
   private static VisionProcessing instance=null;
   
   public static VisionProcessing getInstance(){
       if(instance==null){
           instance = new VisionProcessing();
       }
       return instance;
   }
   
   private VisionProcessing(){
    camera = AxisCamera.getInstance();  // get an instance ofthe camera
    cc = new CriteriaCollection();      // create the criteria for the particle filter
    cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 30, 400, false);
    cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, 400, false);
    
        new Thread(this).start();
    }
    
    public void run(){
        if(CommandBase.robot_global.visionEnabled()==true){
        try {
               ColorImage image = null;     
            try {
                image = camera.getImage();
            } catch (AxisCameraException ex) {
       } catch (NIVisionException ex) {
                }
                BinaryImage thresholdImage = image.thresholdRGB(25, 255, 0, 45, 0, 47);   // keep only red objects
                BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts
                BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
                BinaryImage filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles
                
                ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();  // get list of results
                for (int i = 0; i < reports.length; i++) {                                // print results
                    ParticleAnalysisReport r = reports[i];
                    System.out.println("Particle: " + i + ":  Center of mass x: " + r.center_mass_x);
                    if(i == 0){
                    coordinates[0] = r.center_mass_x;
                    coordinates[1]= r.center_mass_y;
                    }
                }
                System.out.println(filteredImage.getNumberParticles() + "  " + Timer.getFPGATimestamp());

                
                filteredImage.free();
                convexHullImage.free();
                bigObjectsImage.free();
                thresholdImage.free();
                image.free();
                
//            } catch (AxisCameraException ex) {        // this is needed if the camera.getImage() is called
//                ex.printStackTrace();
            } catch (NIVisionException ex) {
                
            }
            try {
                Thread.sleep(150);
            } catch (InterruptedException ex) {
                
            }
        }
        else{
        try {
            Thread.sleep(150);
        } catch (InterruptedException ex) {
            
        }
        }}
    
    /**
     *gets the target coordinates
     * @return
     * index 0 has x value, index 1 has y value
     */
    public double[] getTargetCoordinates(){
        return coordinates;
        }
    public double getTargetXVal(){
        return coordinates[0];
    }
    }

    
   

